﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace PID
{
    class PID
    {
        private double _kp;
        private double _ki;
        private double _kd;
        private double _dt;
        private double[] _iTerm;
        private Int64 _sampleCount;
        private int _iSamples;
        private double _lastValue;

        private double _minOutput;
        private double _maxOutput;

        #region Constructor
        /// <summary>
        /// PID Constructor.
        /// </summary>
        /// <param name="Kp">Proportional gain.</param>
        /// <param name="Ki">Intergral gain.</param>
        /// <param name="Kd">Differential gain.</param>
        /// <param name="SampleTime">Period between samples (ms).</param>
        public PID(double Kp, double Ki, double Kd, double SampleTime = 1000.0, int IntergralSampleCount = 25, double MaxOutput = 100, double MinOutput = 0)
        {
            _dt = SampleTime / 1000.0;
            _kp = Kp;
            _ki = Ki * _dt;
            _kd = Kd / _dt;

            _iSamples = IntergralSampleCount;
            _iTerm = new double[_iSamples];
            _sampleCount = 0;

            _lastValue = 0;

            _maxOutput = MaxOutput;
            _minOutput = MinOutput;
        }

        public PID() : this(1, 0, 0)
        {
        }
        #endregion

        /// <summary>
        /// Compute the next sample of the PID.
        /// </summary>
        /// <param name="SetPoint">The desired operating point</param>
        /// <param name="ProcessValue">The actual operating point</param>
        /// <returns></returns>
        public double Compute(double SetPoint, double ProcessValue)
        {
            int count = (int)(_sampleCount++ % _iSamples);
            double error = (SetPoint - ProcessValue);
            double dTerm = (ProcessValue - _lastValue);
            double iTerm = 0;
            double output = 0;

            _iTerm[count] = _ki * error;

            // Compute bandlimited intergral value
            foreach (double ival in _iTerm)
                iTerm += (ival / _iSamples);

            // Calculate the result
            output = _kp * error + iTerm - _kd * dTerm;

            // Store the process value for next compute cycle
            _lastValue = ProcessValue;
            return output;
        }

        public double Kp { get { return this._kp; } set { this._kp = value; } }
        public double Ki { get { return this._ki; } set { this._ki = value; } }
        public double Kd { get { return this._kd; } set { this._kd = value; } }
        public int SampleTime { get { return (int)(this._dt * 1000); } set { this._dt = value / 1000.0; } }
        public int IntergralSamples { get { return this._iSamples; } set { this._iSamples = value; } }
    }
}
